#include "custom_filters/MeanFilter.hpp"

MeanFilter::MeanFilter()
{
  RCLCPP_INFO(rclcpp::get_logger("MeanFilter"), "Blank constructor of MeanFilter");
}

void MeanFilter::set_filter_time(double time)
{
  filter_time = time;
}

double MeanFilter::get_filter_time()
{
  return filter_time;
}

void MeanFilter::add_data(double data)
{
  rclcpp::Time now_stamp = rclcpp::Clock().now();

  data_list.push_back(data_t(data, now_stamp));
  if ((now_stamp - data_list.front().stamp).seconds() > filter_time && data_list.size() >= 1)
  {
    data_list.pop_front();
  }
}

void MeanFilter::reset()
{
  data_list.clear();
}

double MeanFilter::get_sum()
{
  double sum = 0;
  for (size_t i = 0; i < data_list.size(); i++)
    sum += data_list[i].data;
  return sum;
}

double MeanFilter::get_mean()
{
  if (data_list.size() > 0)
    return get_sum() / data_list.size();
  else
  {
    RCLCPP_WARN(rclcpp::get_logger("MeanFilter"), "No data in filter");
    return 0.0;
  }
}

void MeanFilter::update_covariance()
{
  double mean = get_mean();
  variance = 0; // Reset variance before calculating

  for (size_t i = 0; i < data_list.size(); i++)
  {
    variance += (data_list[i].data - mean) * (data_list[i].data - mean);
  }
  variance /= data_list.size();
}

double MeanFilter::apply_filter(double input)
{
  add_data(input);
  update_covariance();
  return get_mean();
}

double MeanFilter::get_variance()
{
  return variance;
}